#include "actuator/actuator.h"

using namespace actuator;

ActuatorMain::ActuatorMain(ros::NodeHandle &nh, ros::NodeHandle &private_nh, std::string param_file, std::string log_path)
: nh_(nh),
  private_nh_(private_nh),
  param_file_(param_file),
  log_path_(log_path)
{
    ptr_kinematic_ = std::make_shared<AMRKinematicModeNormal>();
    // Init();
    InitLog();
}

ActuatorMain::~ActuatorMain()
{
    
}

void ActuatorMain::InitLog()
{
    // common_lib::SpdLog::Instance()->InitSpdLog(log_path_ + "/log", "actuator"); txt_log
    // TsdpClientLog::GetInstance().Init(); // TODO CSVlogForLater
}

void ActuatorMain::Init(std::string file)
{

}

// 输入控制命令，进行运动学模型解算。输入反馈状态，进行逆运动学模型解算

void ActuatorMain::ActuatorSendHandler(AMRSendCmd &amr_cmd, AMRReceiveState &amr_state, ControlVelocity &ctrl_velocity)
{
    AMRCmdAndState cmd_state;
    cmd_state.steer_cmd.clear();
    cmd_state.steer_param = amr_state.steer_param;
    cmd_state.steer_state = amr_state.steer_state;

    // printf("ActuatorHandler(): v: %f, w: %f, a: %f, mode: %d\n", ctrl_velocity.speed, ctrl_velocity.omega, ctrl_velocity.speed_toward, ctrl_velocity.mode);
    ptr_kinematic_->KinematicMode(ctrl_velocity.speed, ctrl_velocity.omega, ctrl_velocity.speed_toward, ctrl_velocity.mode, cmd_state);

    amr_cmd.move_mode = cmd_state.move_mode;
    amr_cmd.steer_cmd = cmd_state.steer_cmd;    
}

void ActuatorMain::ActuatorRecvHandler(AMRSendCmd &amr_cmd, AMRReceiveState &amr_state, VehicleVelocity &vehicle_velocity)
{
    AMRCmdAndState cmd_state;
    cmd_state.steer_cmd.clear();
    cmd_state.steer_param = amr_state.steer_param;
    cmd_state.steer_state = amr_state.steer_state;

    ptr_kinematic_->VehicleInverseMode(cmd_state, vehicle_velocity.speed, vehicle_velocity.omega, vehicle_velocity.speed_toward);
    // printf("ActuatorHandler Inverse: v: %f, w: %f, a: %f\n", vehicle_velocity.speed, vehicle_velocity.omega, vehicle_velocity.speed_toward);
}
